/**
 * 
 */
package robocup.component.worldmodel;

import java.util.Arrays;

import robocup.component.Prophet;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.ControllerNumbers.TEAM;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.InfoReceiver;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.PlayerTypesInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.math.PermutationEvaluator;
import robocup.component.speechacts.MessageFactory.MESSAGES;
import robocup.component.tactics.DefaultPositions;
import robocup.component.worldobjects.Player;

/**
 * This model matches each player to a role based on the players position.
 * 
 * @author wetzker
 * 
 * TODO
 * 
 * add defense players add more final positions
 */
public class RoleModel implements InfoReceiver {

	// the matches the index is the player number
	// the value the role
	// number = index, role = value
	private final int[] rolesAsNumbers = new int[11];

	// role = index, number = value
	// used for reverse lookup
	private final int[] numbersAsRoles = new int[11];

	// the matrix that will be used for the matching
	// of players against roles
	private final double[][] matchingMatrix = new double[6][6];

	// used in the matching recursion (see there)
	private final int[] wayTaken = new int[6];

	private final int[] bestWay = new int[6];

	private final double[] sums = new double[3];

	private final boolean[] columnUsed = new boolean[6];

	// the matrix for the defense
	private final double[][] defenseMatrix = new double[4][4];

	// used in the matching recursion for defense (see there)
	private final int[] wayTaken2 = new int[4];

	private final int[] bestWay2 = new int[4];

	private final double[] sums2 = new double[2];

	private final boolean[] columnUsed2 = new boolean[4];

	private final WorldModel world;

	private final Prophet prophet;

	// values that are stored for determing if the
	// world has changed enough to recalculate the model
	private TEAM lastPossesionOfBall = TEAM.WE;

	private final Vektor lastIP = Vektor.getDummyInstance();

	private Player lastPlayerWithBall = null;

	private int lastUpdateCycle = -1;

	private final double ipChangeDistance = 15.0;

	private final int oldWorldAge = 20;

	private final boolean DEBUG = false;
	
	//private Vektor dummyVektor = Vektor.getDummyInstance();

	// max iterations for mapping (unused...)
	private int maxIterations = Integer.MAX_VALUE;

	/**
	 * Constructor.
	 * 
	 * @param world -
	 *          the world
	 * @param prophet -
	 *          the prophet
	 */
	public RoleModel(final WorldModel world, final Prophet prophet) {

		this.world = world;
		this.prophet = prophet;
		this.lastPlayerWithBall = world.getAllPlayers()[0];

		for (int i = 0; i < 11; i++) {
			this.rolesAsNumbers[i] = i + 1;
			this.numbersAsRoles[i] = i + 1;
		}
	}

	/**
	 * updates the model
	 */
	public void update() {

		// only update when changes occured
		if (!SConf.getInstance().COACH
				&& this.prophet.getParent().getSynchro().getVIArrivalQuarter() > 1
				&& this.worldHasChanged() && !this.world.canKick()) {
			this.reset();

			// if(this.prophet.inPossessionOfBall() != TEAM.OPPONENT){
			// this.fillMatrix(DefaultPositions.finalPositionsAttacking);
			// }
			// else{
			// this.fillMatrix(DefaultPositions.finalPositionsDefense);
			// }
			// test: use only offensive final-positions!
			this.fillMatrix(DefaultPositions.finalPositionsAttacking);

			//this.shakeMatrix();
			this.filterMatrix();
			this.mapPlayers(0, this.wayTaken, this.bestWay, this.sums,
					this.columnUsed);

			if (this.sums[2] >= this.maxIterations) {
				return;
			}

			for (int i = 0; i < this.bestWay.length; i++) {
				this.bestWay[i] = this.bestWay[i] + 6;
			}

			System.arraycopy(this.bestWay, 0, this.rolesAsNumbers, 5,
					this.bestWay.length);
			this.setRolesToPlayers();

			if (this.DEBUG) {
				System.out
						.println(this.world.getCycleAndPlayerString()
								+ " RoleModel updated:"
								+ Arrays.toString(this.bestWay));
			}

			// remember the world state
			this.lastIP.copy(this.prophet.getNextBallInterceptionPoint());
			this.lastPlayerWithBall = this.prophet.getFastestTeammateToBall();
			this.lastUpdateCycle = this.world.getBodyCycle();
			this.lastPossesionOfBall = this.prophet.inPossessionOfBall();

			this.setRoleNumbers();
		}
		if (this.world.getNumber() > 1 && this.world.getNumber() < 6) {
			this.reset();
			this.fillMatrixDefense(DefaultPositions.finalPositionsDefense);
			PermutationEvaluator
					.smallestPermutation(this.defenseMatrix, 0, this.wayTaken2,
							this.bestWay2, this.sums2, this.columnUsed2);
			for (int i = 0; i < this.bestWay2.length; i++) {
				this.bestWay2[i] = this.bestWay2[i] + 2;
			}
			System.arraycopy(this.bestWay2, 0, this.rolesAsNumbers, 1,
					this.bestWay2.length);
			this.setRolesToPlayers();
		}
	}

	/**
	 * @return
	 */
	private boolean worldHasChanged() {

		if (!this.world.isViBased()
				|| this.prophet.getFastestTeammateToBall() == null
				|| this.prophet.getNextBallInterceptionPoint() == null
				|| this.world.getNumber() < 6) {
			return false;
		}
		
		if(this.world.getPlayMode() != PLAY_MODE.PLAY_ON){
			return true;
		}

		final boolean possessionChanged = this.lastPossesionOfBall != this.prophet
				.inPossessionOfBall();
		final boolean playerWithBallChanged = (this.prophet
				.inPossessionOfBall() == TEAM.WE)
				&& this.prophet.getFastestTeammateToBall().getNumber() != this.lastPlayerWithBall
						.getNumber();
		final boolean ipChanged = this.prophet.getNextBallInterceptionPoint()
				.getDistance(this.lastIP) > this.ipChangeDistance;
		final boolean oldWorld = this.world.getBodyCycle()
				- this.lastUpdateCycle > this.oldWorldAge;
		final boolean kicked = this.world.getLastHeardMessageType() == MESSAGES.KICKED_BALL;

		return possessionChanged || playerWithBallChanged || ipChanged
				|| oldWorld || kicked;
	}

	/**
	 * resets all values
	 */
	private void reset() {

		this.sums[0] = 0;
		this.sums[1] = 1000000;
		this.sums[2] = 0;

		Arrays.fill(this.columnUsed, false);

		this.sums2[0] = 0;
		this.sums2[1] = 1000000;

		Arrays.fill(this.columnUsed2, false);

		Arrays.fill(this.bestWay, Integer.MAX_VALUE);
	}

	/**
	 * fills the match matrix given the final positions.
	 * 
	 * @param finalPositions -
	 *          the positions to optimize against (attack positions when in ball
	 *          possession, defense positions otherwise)
	 */
	private void fillMatrix(final Vektor[] finalPositions) {

		Vektor playerPos;
		boolean[] nearTeammates = this.getNearTeammates();
		
		double rejectionFactor;
		
		int myIndex = this.world.getNumber() -1;
		
		// for all players and positions
		for (int playerIndex = 5; playerIndex < 11; playerIndex++) {
			for (int positionIndex = 5; positionIndex < 11; positionIndex++) {
				if (this.world.getExpectedPlayers()[playerIndex]
						.equals(this.prophet.getFastestPlayerToBall())) {
					playerPos = this.prophet.getNextBallInterceptionPoint();
				} else {
					playerPos = this.world.getExpectedPlayers()[playerIndex]
							.getPosition();
				}
				
				rejectionFactor = (nearTeammates[playerIndex-5]) ? -6 : 0;

				if(playerIndex < myIndex){
					rejectionFactor = (nearTeammates[playerIndex-5]) ? 6 : 0;
				}

				this.matchingMatrix[playerIndex - 5][positionIndex - 5] = 
					Math.pow(
							Math.max(playerPos.getDistance(finalPositions[positionIndex]) + rejectionFactor,0),
								2.0);
			}
		}
	}
	
	/**
	 * @return a boolean array. true if an teammate is near and has a lower number.
	 * false otherwise.
	 */
	private boolean[] getNearTeammates(){
		boolean[] isNear = {false,false,false,false,false,false};
		
		for(int i=5; i<11;i++){
			Player p = this.world.getExpectedPlayers()[i];
			if(!p.isMe()){
				if(p.getDistance(this.world.getSelfRef()) < 6){
					isNear[i-5] = true;
				}
			}
		}
		
		return isNear;
	}

	/**
	 * sometimes 2 players fit equally well to a position. in this case the player
	 * with the lower number is set farer away to get away from the equilibrium.
	 * Avoids player clustering.
	 */
	private void shakeMatrix() {

		double minValue;
		double minValue2;
		double value;

		for (int playerIndex = 5; playerIndex < 11; playerIndex++) {
			minValue = Integer.MAX_VALUE;
			minValue2 = Integer.MAX_VALUE;

			// set min and 2nd min values
			for (int positionIndex = 5; positionIndex < 11; positionIndex++) {
				value = this.matchingMatrix[playerIndex - 5][positionIndex - 5];

				if (value < minValue) {
					minValue2 = minValue;
					minValue = value;
				} else if (value < minValue2) {
					minValue2 = value;
				}
			}

			// if 2 players fit to 1 position, set the first (virtually) away
			if (minValue > minValue2 - 5) {
				for (int positionIndex = 5; positionIndex < 11; positionIndex++) {
					value = this.matchingMatrix[playerIndex - 5][positionIndex - 5];
					if (value < minValue2 + 0.1) {
						this.matchingMatrix[playerIndex - 5][positionIndex - 5] = this.matchingMatrix[playerIndex - 5][positionIndex - 5] + 5;
					}
				}
			}
		}

	}

	/**
	 * the player with ball is given possible roles. all other roles are filtered
	 * for this player.
	 */
	private void filterMatrix() {

		final Player playerWithBall = this.prophet.getFastestTeammateToBall();
		if (playerWithBall.getNumber() < 6) {
			return;
		}

		final Vektor ip = this.prophet.getNextBallInterceptionPoint();

		final boolean[] roles = new boolean[11];

		Arrays.fill(roles, false);

		final double angleToGoalPostLeft = ip
				.getAngleTo(SConf.getInstance().GOAL_POST_LEFT_OTHER);
		final double angleToGoalPostRight = ip
				.getAngleTo(SConf.getInstance().GOAL_POST_RIGHT_OTHER);

		// if near corner only the wing attackers are allowed
		if (angleToGoalPostLeft > 60) {
			roles[8] = true;
		} else if (angleToGoalPostRight < -60) {
			roles[10] = true;
		}
		// if on the side only the wing players are allowed
		else if (ip.x < -24) {
			roles[5] = true;
			roles[8] = true;
		} else if (ip.x > 24) {
			roles[7] = true;
			roles[10] = true;
		}
		// if slightly to one side, the opposite wing players are excluded
		else if (ip.x < 0) {
			roles[5] = true;
			roles[6] = true;
			roles[8] = true;
			roles[9] = true;
		} else if (ip.x > 0) {
			roles[6] = true;
			roles[7] = true;
			roles[9] = true;
			roles[10] = true;
		}
		// otherwise the ball player could have all roles
		else {
			roles[5] = true;
			roles[6] = true;
			roles[7] = true;
			roles[8] = true;
			roles[9] = true;
			roles[10] = true;
		}

		for (int positionIndex = 5; positionIndex < 11; positionIndex++) {

			// only attackers are near the opponents goal line
			if (ip.y + 5 > SConf.getInstance().GOAL_POS_OTHER.y
					&& positionIndex < 8) {
				roles[positionIndex] = false;
			}

			if (!roles[positionIndex]) {
				this.matchingMatrix[playerWithBall.getNumber() - 5 - 1][positionIndex - 5] = -1;
			}
		}

	}

	/**
	 * fills the match matrix given the final positions.
	 * 
	 * @param finalPositions -
	 *          the positions to optimize against (attack positions when in ball
	 *          possession, defense positions otherwise)
	 */
	private void fillMatrixDefense(final Vektor[] finalPositions) {

		Vektor playerPos;

		// for all players and positions
		for (int playerIndex = 1; playerIndex < 5; playerIndex++) {
			for (int positionIndex = 1; positionIndex < 5; positionIndex++) {
				playerPos = this.world.getExpectedPlayers()[playerIndex]
						.getPosition();
				this.defenseMatrix[playerIndex - 1][positionIndex - 1] = Math
						.pow(Math.abs(playerPos.x
								- finalPositions[positionIndex].x), 2.0);
			}
		}
	}

	/**
	 * recursive method that optimizes the matches based on the formular:
	 *  -> min(Sum(distance(Px, matchedPosition(Px)) ^2)) for all players
	 * 
	 * @param rowIdx -
	 *          the row index (starting with 0)
	 * @param wayTaken -
	 *          the way in the matrix that was used to reach the current point
	 * @param bestWay -
	 *          the way that lead to the smallest sum
	 * @param sums -
	 *          sum[0] the current sum, 
	 *          sum[1] the best sum found so far, 
	 *          sum[2] the number of iterations
	 * @param columnUsed -
	 *          all columns that wer already used are set to false
	 */
	public void mapPlayers(final int rowIdx, final int[] wayTaken,
			final int[] bestWay, final double[] sums, final boolean[] columnUsed) {

		sums[2] += 1;

		// reached a leaf -> recursion ends here
		if (rowIdx == this.matchingMatrix.length
				|| sums[2] > this.maxIterations) {
			if (sums[0] < sums[1]) {
				sums[1] = sums[0];
				System.arraycopy(wayTaken, 0, bestWay, 0, bestWay.length);
			}

			return;
		}

		final double currentSum = sums[0];

		// go ahead for all nodes
		for (int j = 0; j < this.matchingMatrix[rowIdx].length; j++) {
			if (this.matchingMatrix[rowIdx][j] >= 0) {
				// skip used columns
				if (columnUsed[j]) {
					continue;
				}

				final double newSum = currentSum
						+ this.matchingMatrix[rowIdx][j];

				// as some ways in the tree are obviously bad skip them
				if (newSum > sums[1]) {
					continue;
				} else {
					columnUsed[j] = true;
					wayTaken[rowIdx] = j;
					sums[0] = newSum;

					// do it again
					this.mapPlayers(rowIdx + 1, wayTaken, bestWay, sums,
							columnUsed);
					columnUsed[j] = false;
				}
			} else {
				continue;
			}
		}
	}

	/**
	 * sets the calculated roles to each player.
	 */
	private void setRolesToPlayers() {

		for (int i = 0; i < this.rolesAsNumbers.length; i++) {

			if (this.DEBUG
					&& this.world.getAllPlayers()[i].isMe()
					&& this.world.getAllPlayers()[i].getRoleNumber() != this.rolesAsNumbers[i]) {
				System.out.println(this.world.getCycleAndPlayerString()
						+ " Change Role from "
						+ this.world.getAllPlayers()[i].getRoleNumber()
						+ " to " + (this.rolesAsNumbers[i]));
			}

			this.world.getAllPlayers()[i].setRoleNumber(this.rolesAsNumbers[i]);
			this.world.getExpectedPlayers()[i]
					.setRoleNumber(this.rolesAsNumbers[i]);
			this.world.getNextPlayers()[i]
					.setRoleNumber(this.rolesAsNumbers[i]);
		}
	}

	/**
	 * the inverse of the rolesAsNumbers (index and value are switched) makes it
	 * easier to tell which roles is occupied by which player
	 */
	private void setRoleNumbers() {

		for (int i = 0; i < this.rolesAsNumbers.length; i++) {
			this.numbersAsRoles[this.rolesAsNumbers[i] - 1] = i + 1;
		}

	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.SenseBodyInfo)
	 */
	public void update(final SenseBodyInfo info) {

		// TODO Auto-generated method stub

	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.VisualInfo)
	 */
	public void update(final VisualInfo info) {

		this.update();

	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.RefereeInfo)
	 */
	public void update(final RefereeInfo info) {

		// TODO Auto-generated method stub

	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayModeInfo)
	 */
	public void update(final PlayModeInfo info) {

		// TODO Auto-generated method stub

	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayerSayInfo)
	 */
	public void update(final PlayerSayInfo info) {

		// this.update();
	}

	/**
	 * returns the final position based on a players role and the ball possession
	 * 
	 * @param roleNumber -
	 *          a players role number
	 * @param defense -
	 *          defense or offense
	 * @return final position of the player in the given mode
	 */
	public static Vektor getFinalPosition(final int roleNumber,
			final boolean defense) {

		if (roleNumber < 1 || roleNumber > 11) {
			if (defense) {
				return SConf.getInstance().GOAL_POS_OWN;
			} else {
				return SConf.getInstance().GOAL_POS_OTHER;
			}
		}

		if (defense) {
			return DefaultPositions.finalPositionsDefense[roleNumber - 1];
		} else {
			return DefaultPositions.finalPositionsAttacking[roleNumber - 1];
		}
	}

	/**
	 * simple getter
	 * 
	 * @param role
	 * @return
	 */
	public int getNumberForRole(final int role) {

		return this.rolesAsNumbers[role - 1];
	}

	/* (non-Javadoc)
	 * @see robocup.component.infotypes.InfoReceiver#update(robocup.component.infotypes.PlayerTypesInfo)
	 */
	public void update(final PlayerTypesInfo info) {

		// TODO Auto-generated method stub

	}

}
